﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

using System.Timers;
using System.IO.Ports;
using System.Threading;
using System.Diagnostics;


namespace VehicleTest
{
    public enum ReceiveState
    {
        VehiclePacket,
        LMS111DataPacket,
        LMS111SettingPacket,
        OnlyServoPacket
    }

    public partial class MainWindows : Form
    {
        private FollowerVehicle followervehicle;
        private LMS111_Test lms111;
        private Calibration cali;
        private VehicleDetection vehicle_following;
        private VehicleCommand vehicle_mw;
        private SerialSettings serialsettings;
        public bool serial_receive_on = false;
        public SerialPort serialport = new SerialPort();
        private System.Timers.Timer time_count;
        //获取执行文件所在的路径
        public string path;
        //绘图
        public delegate void TrackShowDelegate(double m, Int32 n, int i, uint a);
        public delegate void TrackDrawDelegate(double x, double y, Int32 n, int i, uint value);
        private System.Timers.Timer vehicle_track;
        private double XMAX, YMAX, XMIN, YMIN;
        //private Int32 test_curve;
        //private bool flag_test = false;
        private bool flag_ok_no;
        private bool flag_sendmore;
        private System.Timers.Timer sendmore;
        //串口
        public string PortName;
        public string BandRate;
        public string Parity;
        public string StopBit;
        public string DataBits;
        public string DataMode;
        public volatile byte[] data_receive;
        public volatile int data_length;
        public volatile int data_length_temp;
        private volatile bool flag_serial_open;
        //小车返回数据
        public double follower_x_coordinate;
        private double follower_x_coordinate_temp;
        public double follower_y_coordinate;
        private double follower_y_coordinate_temp;
        public double speed;
        public double angle;
        public bool FollowerVehicle_open;
        //前车坐标
        public double leader_x_coordinate;
        public double leader_y_coordinate;
        private double leader_x_coordinate_temp;
        private double leader_y_coordinate_temp;
        private double leader_x_coordinate_temp2;
        private double leader_y_coordinate_temp2;
        //LMS111返回数据
        public int[] LMS_data;
        public int LMS_NUM = 241;
        public bool LMS111_open;

        public volatile ReceiveState receive_state = ReceiveState.VehiclePacket;
        public volatile bool flag_ASCII_r = true;
        private volatile bool flag_communicate_show = false;
        private volatile bool flag_communicate_green = false;
        //private Thread _readSerialport;

        public MainWindows()
        {
            InitializeComponent();

            path = System.Windows.Forms.Application.StartupPath;

            vehicle_mw = new VehicleCommand(this);

            ToolStripMenuItem_SerialOpen.Enabled = false;
            ToolStripMenuItem_test.Enabled = false;
            toolStripButton_send.Enabled = false;
            toolStripButton_sendMore.Enabled = false;
            toolStripButton_Reset.Enabled = false;
            flag_ok_no = false;
            flag_sendmore = false;
            LMS111_open = false;
            FollowerVehicle_open = false;
            
            data_receive = new byte[255];
            flag_serial_open = false;

            time_count = new System.Timers.Timer();
            time_count.Interval = 200;
            time_count.Enabled = Enabled;
            time_count.Stop();
            time_count.Elapsed += new ElapsedEventHandler(Time_count_tick);

            vehicle_track = new System.Timers.Timer();
            vehicle_track.Interval = 50;
            vehicle_track.Enabled = Enabled;
            vehicle_track.Stop();
            vehicle_track.Elapsed += new ElapsedEventHandler(VehicleTrack_tick);

            sendmore = new System.Timers.Timer();
            sendmore.Interval = 150;
            sendmore.Enabled = Enabled;
            sendmore.Stop();
            sendmore.Elapsed += new ElapsedEventHandler(sendmore_tick);

            LMS_data = new int[LMS_NUM];

            Chart_track_Init();
        }

        #region   自定义函数
        #region   绘图函数
        private void Chart_track_Init()
        {
            chart_VehicleFollowing.ChartAreas[0].AxisX.Maximum = Convert.ToDouble(toolStripTextBox_x_max.Text);
            chart_VehicleFollowing.ChartAreas[0].AxisX.Minimum = Convert.ToDouble(toolStripTextBox_x_min.Text);
            chart_VehicleFollowing.ChartAreas[0].AxisY.Maximum = Convert.ToDouble(toolStripTextBox_y_max.Text);
            chart_VehicleFollowing.ChartAreas[0].AxisY.Minimum = Convert.ToDouble(toolStripTextBox_y_min.Text);

            XMAX = chart_VehicleFollowing.ChartAreas[0].AxisX.Maximum;
            XMIN = chart_VehicleFollowing.ChartAreas[0].AxisX.Minimum;
            YMAX = chart_VehicleFollowing.ChartAreas[0].AxisY.Maximum;
            YMIN = chart_VehicleFollowing.ChartAreas[0].AxisY.Minimum;

            chart_VehicleFollowing.Series[0].Points.AddXY(0, 0);
        }

        #endregion

        public void OpenSerialOpen(bool a)
        {
            if(a == true)
            {
                ToolStripMenuItem_SerialOpen.Enabled = true;
            }
            else
            {
                ToolStripMenuItem_SerialOpen.Enabled = false;
            }
        }

        private void EnableControls()
        {
            ToolStripMenuItem_test.Enabled = true;
            toolStripButton_send.Enabled = true;
            toolStripButton_sendMore.Enabled = true;
            toolStripButton_Reset.Enabled = true;

            if (!(vehicle_following == null || vehicle_following.IsDisposed))
            {
                vehicle_following.toolStripButton_OpenControl.Enabled = true;
            }
        }

        public byte[] HexStringToByteArray(string s)
        {
            s = s.Replace(" ", "");
            byte[] buffer = new byte[s.Length / 2];
            for (int i = 0; i < s.Length; i += 2)
                buffer[i / 2] = (byte)Convert.ToByte(s.Substring(i, 2), 16);
            return buffer;
        }

        public string ByteArrayToHexString(byte[] data)
        {
            StringBuilder sb = new StringBuilder(data.Length * 3);
            foreach (byte b in data)
                sb.Append(Convert.ToString(b, 16).PadLeft(2, '0').PadRight(3, ' '));
            return sb.ToString().ToUpper();
        }

        public string ByteArrayToHexString_length(byte[] data, int length)
        {
            StringBuilder sb = new StringBuilder(length * 3);
            for (int a = 0; a < length; a++)
            {
                sb.Append(Convert.ToString(data[a], 16).PadLeft(2, '0').PadRight(3, ' '));
            }
            return sb.ToString().ToUpper();
        }

        public byte checksum(byte[] bRx)
        {
            byte sum = 0;
            for (int i = 0; i < bRx.Length; i++)
            {
                sum += bRx[i];
            }
            sum = (byte)(~((int)sum));
            return sum;
        }

        public void SerialSend(string serialdata)
        {
            //if (_readSerialport.ThreadState == (ThreadState.Suspended | ThreadState.Background))
            //{
            //    _readSerialport.Resume();
            //}
            byte[] data = HexStringToByteArray(serialdata);
            serialport.Write(data, 0, data.Length);
        }

        public void SerialSend1(byte[] data)
        {
            serialport.Write(data, 0, data.Length);
        }

        public void SerialSend2(byte[] data, int length)
        {
            serialport.Write(data, 0, length);
        }

        public string ConvertToASCII_One(byte b)
        {
            string s = null;

            s = ((char)b).ToString();
            return s;
        }

        public string ConvertToASCII_More(byte[] b)
        {
            string s = null;
            int length = b.Length;
            for (int i = 0; i < length; i++)
            {
                s += ((char)b[i]).ToString();
            }
            return s;
        }

        private void PreClosing()
        {
            vehicle_track.Stop(); //为了防止绘图没有停止就关闭,会出现异常
            vehicle_track.Close();

            Thread.Sleep(100);

            if (followervehicle != null)
                if (!followervehicle.IsDisposed)
                    followervehicle.PreClosing();

            if (cali != null)
                if (!cali.IsDisposed)
                    cali.PreClosing();

            if (vehicle_following != null)
                if (!vehicle_following.IsDisposed)
                    vehicle_following.PreClosing();

            //this.Controls.Clear(); //为了防止RCW错误
        }

        delegate void EventFormClosing(FormClosingEventArgs e);
        protected override void OnFormClosing(FormClosingEventArgs e)
        {
            //关闭前的准备工作
            PreClosing();
            this.Dispose();
            base.OnFormClosing(e);   //在MSDN中规定如果重载了OnFormClosing函数，最后要调用基类的OnFormClosing函数
        }
        #endregion

        #region 控件事件
        private void ToolStripMenuItem_Exit_Click(object sender, EventArgs e)
        {
            //Application.Exit();
            PreClosing();
            this.Dispose();
        }

        private void ToolStripMenuItem_FollowerTest_Click(object sender, EventArgs e)
        {
            if ((followervehicle == null) || (followervehicle.IsDisposed))
            {
                followervehicle = new FollowerVehicle(this);
            }

            followervehicle.Show();
        }

        private void ToolStripMenuItem_SerialSettings_Click(object sender, EventArgs e)
        {
            if ((serialsettings == null) || (serialsettings.IsDisposed))
            {
                serialsettings = new SerialSettings(this);
            }

            serialsettings.Show();
        }

        private void ToolStripMenuItem_lMS111_Click(object sender, EventArgs e)
        {
            if ((lms111 == null) || (lms111.IsDisposed))
            {
                lms111 = new LMS111_Test(this);
            }

            lms111.Show();
        }

        private void ToolStripMenuItem_SerialOpen_Click(object sender, EventArgs e)
        {
            try
            {
                if (serialport.IsOpen)
                {
                    if (!(vehicle_following == null || vehicle_following.IsDisposed)) //为了确保关闭控制串口后,控制界面释放资源
                    {
                        vehicle_following.toolStripButton_OpenControl.Text = "打开控制";
                        vehicle_following.toolStripButton_OpenControl.Enabled = false;
                        if (!(vehicle_following.vehicle_control == null || vehicle_following.vehicle_control.IsDisposed))
                        {
                            vehicle_following.vehicle_control.control_period.Stop();
                            vehicle_following.vehicle_control.Dispose();
                        }
                    }
                    serial_receive_on = false;
                    serialport.Close();
                    flag_serial_open = false;
                    ToolStripMenuItem_SerialOpen.Text = "打开串口";
                    toolStripStatusLabel_state_series.Image = Image.FromFile(path + "/image/red.jpg");
                    ToolStripMenuItem_test.Enabled = false;
                    toolStripButton_send.Enabled = false;
                    toolStripButton_sendMore.Enabled = false;
                    toolStripButton_Reset.Enabled = false;
                    ToolStripMenuItem_SerialSettings.Enabled = true;
                    time_count.Stop();
                }
                else
                {
                    serial_receive_on = true;
                    serialport.PortName = PortName;
                    serialport.BaudRate = int.Parse(BandRate);
                    serialport.DataBits = int.Parse(DataBits);
                    serialport.StopBits = (StopBits)Enum.Parse(typeof(StopBits), StopBit);
                    serialport.Parity = (Parity)Enum.Parse(typeof(Parity), Parity);

                    serialport.DataReceived += new SerialDataReceivedEventHandler(event_ReadSerialport);
                    serialport.Open();
                    flag_serial_open = true;
                    //_readSerialport = new Thread(ReadSerialport);
                    //_readSerialport.IsBackground = true;
                    //_readSerialport.Start();

                    EnableControls();
                    ToolStripMenuItem_SerialSettings.Enabled = false;
                    ToolStripMenuItem_SerialOpen.Text = "关闭串口";
                    toolStripStatusLabel_state_series.Image = Image.FromFile(path + "/image/green.jpg");
                    time_count.Start();

                }
            }
            catch (System.UnauthorizedAccessException ex)
            {
                MessageBox.Show("串口冲突！重新选择可用串口:\n" + ex.Message);
                ToolStripMenuItem_SerialOpen.Enabled = false;
            }
        }

        private void ToolStripMenuItem_Calibration_Click(object sender, EventArgs e)
        {
            if ((cali == null) || (cali.IsDisposed))
            {
                cali = new Calibration(this);
            }

            cali.Show();
        }

        private void ToolStripMenuItem_VehicleFollowing_Click(object sender, EventArgs e)
        {
            if((vehicle_following == null) || (vehicle_following.IsDisposed))
            {
                vehicle_following = new VehicleDetection(this);
            }

            vehicle_following.Show();
        }

        private void toolStripButton_Range_ok_Click(object sender, EventArgs e)
        {
            if (!flag_ok_no)
            {
                vehicle_track.Start();
                toolStripButton_Range_ok.Image = Image.FromFile(path + "/image/stop.png");
                toolStripButton_Range_ok.Text = "停止绘图";
                toolStripStatusLabel_state_draw.Image = Image.FromFile(path + "/image/green.jpg");
                flag_ok_no = true;
                toolStripButton_SaveImage.Enabled = false;
            }
            else
            {
                vehicle_track.Stop();
                toolStripButton_Range_ok.Image = Image.FromFile(path + "/image/start.png");
                toolStripButton_Range_ok.Text = "开始绘图";
                toolStripStatusLabel_state_draw.Image = Image.FromFile(path + "/image/red.jpg");
                flag_ok_no = false;
                toolStripButton_SaveImage.Enabled = true;
            }

        }

        private void toolStripButton_clear_Click(object sender, EventArgs e)
        {
            chart_VehicleFollowing.Series[0].Points.Clear();
            chart_VehicleFollowing.Series[1].Points.Clear();
            chart_VehicleFollowing.Series[0].Points.AddXY(0, 0);
        }

        private void toolStripButton_SaveImage_Click(object sender, EventArgs e)
        {
            SaveFileDialog saveimage = new SaveFileDialog();
            saveimage.Filter = "BMP文件(*.bmp)|*.bmp|JPG文件(*.jpg)|*.jpg";
            if (saveimage.ShowDialog() == DialogResult.OK)
            {
                if (saveimage.FilterIndex == 1)
                    chart_VehicleFollowing.SaveImage(saveimage.FileName, System.Drawing.Imaging.ImageFormat.Bmp);
                else
                    chart_VehicleFollowing.SaveImage(saveimage.FileName, System.Drawing.Imaging.ImageFormat.Jpeg);
            }
        }

        private void toolStripTextBox_x_min_TextChanged(object sender, EventArgs e)
        {
            try
            {
                chart_VehicleFollowing.ChartAreas[0].AxisX.Minimum = Convert.ToDouble(toolStripTextBox_x_min.Text);
                toolStripButton_Range_ok.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_Range_ok.Enabled = false;
            }
            catch (Exception)
            {

            }
            
        }

        private void toolStripTextBox_x_max_TextChanged(object sender, EventArgs e)
        {
            try
            {
                chart_VehicleFollowing.ChartAreas[0].AxisX.Maximum = Convert.ToDouble(toolStripTextBox_x_max.Text);
                toolStripButton_Range_ok.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_Range_ok.Enabled = false;
            }
            catch (Exception)
            {

            }
            
        }

        private void toolStripTextBox_y_min_TextChanged(object sender, EventArgs e)
        {
            try
            {
                chart_VehicleFollowing.ChartAreas[0].AxisY.Minimum = Convert.ToDouble(toolStripTextBox_y_min.Text);
                toolStripButton_Range_ok.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_Range_ok.Enabled = false;
            }
            catch (Exception)
            {

            }
        }

        private void toolStripTextBox_y_max_TextChanged(object sender, EventArgs e)
        {
            try
            {
                chart_VehicleFollowing.ChartAreas[0].AxisY.Maximum = Convert.ToDouble(toolStripTextBox_y_max.Text);
                toolStripButton_Range_ok.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_Range_ok.Enabled = false;
            }
            catch (Exception)
            {

            }
        }

        private void toolStripButton_send_Click(object sender, EventArgs e)
        {
            receive_state = ReceiveState.VehiclePacket;  //准备显示接收数据

            vehicle_mw.VehicleControl((byte)int.Parse(toolStripTextBox_state.Text), int.Parse(toolStripTextBox_speed.Text), 
                                        int.Parse(toolStripTextBox_servo.Text), (byte)int.Parse(toolStripTextBox_light.Text));
        }

        private void toolStripButton_sendMore_Click(object sender, EventArgs e)
        {
            receive_state = ReceiveState.VehiclePacket; //准备显示接收数据

            if (!flag_sendmore)
            {

                toolStripButton_sendMore.Image = Image.FromFile(path + "/image/stop2.jpg");
                toolStripButton_sendMore.Text = "停止连发";
                toolStripStatusLabel_state_sendmore.Image = Image.FromFile(path + "/image/green.jpg");
                sendmore.Start();
                flag_sendmore = true;
            }
            else
            {

                toolStripButton_sendMore.Image = Image.FromFile(path + "/image/send2.jpg");
                toolStripButton_sendMore.Text = "连发";
                toolStripStatusLabel_state_sendmore.Image = Image.FromFile(path + "/image/red.jpg");
                sendmore.Stop();
                flag_sendmore = false;
            }
        }

        private void toolStripButton_Reset_Click(object sender, EventArgs e)
        {
            vehicle_mw.VehicleControl(0x06, 0, 0, 0);
        }

        private void button_clear_DataShow_Click(object sender, EventArgs e)
        {
            richTextBox_DataShow.Clear();
        }

        private void toolStripTextBox_speed_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int value = int.Parse(toolStripTextBox_speed.Text);
                if ((value >= vehicle_mw.SPEED_MIN) && (value <= vehicle_mw.SPEED_MAX) && (flag_serial_open == true))
                {
                    toolStripButton_send.Enabled = true;
                    toolStripButton_sendMore.Enabled = true;
                }
                else
                {
                    toolStripButton_send.Enabled = false;
                    toolStripButton_sendMore.Enabled = false;
                }
            }
            catch (FormatException)
            {
                toolStripButton_send.Enabled = false;
                toolStripButton_sendMore.Enabled = false;
            }
            catch (Exception)
            {
            	
            }
        }

        private void toolStripTextBox_servo_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int value = int.Parse(toolStripTextBox_servo.Text);
                if ((value >= (vehicle_mw.SERVO_MIN - vehicle_mw.SERVO_MID)) && (value <= (vehicle_mw.SERVO_MAX - vehicle_mw.SERVO_MID)) && (flag_serial_open == true))
                {
                    toolStripButton_send.Enabled = true;
                    toolStripButton_sendMore.Enabled = true;
                }
                else
                {
                    toolStripButton_send.Enabled = false;
                    toolStripButton_sendMore.Enabled = false;
                }
            }
            catch (FormatException)
            {
                toolStripButton_send.Enabled = false;
                toolStripButton_sendMore.Enabled = false;
            }
            catch (Exception)
            {

            }
        }

        private void toolStripTextBox_state_TextChanged(object sender, EventArgs e)
        {
            try
            {
                int value = int.Parse(toolStripTextBox_state.Text);
                if (((value == 0) || (value == 1) || (value == 2) || (value == 3)) && (flag_serial_open == true))
                {
                    toolStripButton_send.Enabled = true;
                    toolStripButton_sendMore.Enabled = true;
                }
                else
                {
                    toolStripButton_send.Enabled = false;
                    toolStripButton_sendMore.Enabled = false;
                }
            }
            catch (FormatException)
            {
                toolStripButton_send.Enabled = false;
                toolStripButton_sendMore.Enabled = false;
            }
            catch (Exception)
            {

            }
        }

        private void toolStripTextBox_light_TextChanged(object sender, EventArgs e)
        { 
            try
            {
                int value = int.Parse(toolStripTextBox_light.Text);
                if ((value >= 0) && (value <= 255) && (flag_serial_open == true))
                {
                    toolStripButton_send.Enabled = true;
                    toolStripButton_sendMore.Enabled = true;
                }
                else
                {
                    toolStripButton_send.Enabled = false;
                    toolStripButton_sendMore.Enabled = false;
                }
            }
            catch (FormatException)
            {
                toolStripButton_send.Enabled = false;
                toolStripButton_sendMore.Enabled = false;
            }
            catch (Exception)
            {

            }
        }

        private void ToolStripMenuItem_User_Click(object sender, EventArgs e)
        {
            try
            {
                Process.Start(@"C:\Program Files\Adobe\Acrobat 9.0\Acrobat\Acrobat.exe", path + @"\Help\VehicleTest_HELP.pdf");
            }
            catch (Exception)
            {

            }
            
        }
        #endregion

        #region 触发中断
        private void event_ReadSerialport(object sender, SerialDataReceivedEventArgs e)//使用中……
        {
            int data1 = 0;
            byte data = 0;
            byte data_temp = 0;
            //后车测试变量
            bool flag_start = false;
            int count = 0;
            byte checksum = 0;
            //LMS111变量
            int count1 = 0;  //计空格数
            int count2 = 0;  //计位数
            int count3 = 0;  //计激光点个数
            bool flag_start_lms = false;
            bool flag_data = false;
            byte[] temp = new byte[8];
            //单个舵机测试变量
            bool flag_start_svo = false;
            int count_svo = 0;

            if (serial_receive_on)
            {
                switch (receive_state)
                {
                    case ReceiveState.VehiclePacket:
                        #region   后车测试

                    READ_BYTE: data1 = serialport.ReadByte();

                        data = (byte)data1;

                        if ((data == 0xFF) && (data_temp == 0xFF) && (!flag_start))
                        {
                            data_receive[0] = 0xFF;
                            data_receive[1] = 0xFF;
                            count = 2;
                            flag_start = true;
                        }
                        else if (flag_start)
                        {
                            if (count == 2)
                            {
                                data_length = data + 3;
                                data_receive[count] = data;
                                count++;

                                if (data_length > 30)  //过滤错误数据包,防止错误累积,和读内存的错误！
                                {
                                    flag_start = false;

                                    if (FollowerVehicle_open)
                                    {
                                        followervehicle.richTextBox_show.Invoke(new EventHandler(delegate
                                        {
                                            followervehicle.richTextBox_show.SelectionColor = Color.Red;
                                            followervehicle.richTextBox_show.AppendText("ERROR: 数据包不合理!" + "\n");
                                            followervehicle.richTextBox_show.ScrollToCaret();
                                        }));
                                    }
                                    richTextBox_DataShow.Invoke(new EventHandler(delegate
                                    {
                                        richTextBox_DataShow.SelectionColor = Color.Red;
                                        richTextBox_DataShow.AppendText("ERROR: 数据包不合理!" + "\n");
                                        richTextBox_DataShow.ScrollToCaret();
                                    }));
                                }
                            }
                            else
                            {
                                if (count < data_length)
                                {
                                    data_receive[count] = data;
                                    count++;
                                }
                                if (count == data_length)
                                {
                                    for (int i = 2; i < data_length; i++)
                                    {
                                        checksum += data_receive[i];
                                    }

                                    //数据包检验正确
                                    if (checksum == 0xFF)
                                    {
                                        //数据包长度为16
                                        if (data_length == 16)
                                        {
                                            follower_x_coordinate = (data_receive[4] << 24) + (data_receive[5] << 16) + (data_receive[6] << 8) + data_receive[7];
                                            if ((data_receive[3] & 0x01) == 0x01)
                                            {
                                                follower_x_coordinate = -follower_x_coordinate;
                                            }

                                            follower_y_coordinate = (data_receive[8] << 24) + (data_receive[9] << 16) + (data_receive[10] << 8) + data_receive[11];
                                            if ((data_receive[3] & 0x02) == 0x02)
                                            {
                                                follower_y_coordinate = -follower_y_coordinate;
                                            }

                                            speed = (data_receive[12] << 8) + data_receive[13];
                                            if ((data_receive[3] & 0x04) == 0x04)
                                            {
                                                speed = -speed;
                                            }

                                            angle = data_receive[14] + ((data_receive[3] & 0xF0) << 4);
                                            if ((data_receive[3] & 0x08) == 0x08)
                                            {
                                                angle = -angle;
                                            }
                                        }

                                        //显示所接收的正确数据包
                                        if (FollowerVehicle_open)
                                        {
                                            followervehicle.richTextBox_show.Invoke(new EventHandler(delegate
                                            {
                                                followervehicle.richTextBox_show.SelectionColor = Color.Red;
                                                followervehicle.richTextBox_show.AppendText(ByteArrayToHexString_length(data_receive, data_length) + "\n");
                                                followervehicle.richTextBox_show.ScrollToCaret();
                                            }));
                                        }
                                        richTextBox_DataShow.Invoke(new EventHandler(delegate
                                        {
                                            richTextBox_DataShow.SelectionColor = Color.Red;
                                            richTextBox_DataShow.AppendText(ByteArrayToHexString_length(data_receive, data_length) + "\n");
                                            richTextBox_DataShow.ScrollToCaret();
                                        }));

                                        //计算前车坐标
                                        if (!(vehicle_following == null || vehicle_following.IsDisposed))
                                        {
                                            if (vehicle_following.serial_receive_on)
                                            {
                                                //局部坐标系为运动前方为X轴,左方为Y轴
                                                double x = vehicle_following.target_X; //局部坐标系中的x坐标
                                                double y = vehicle_following.target_Y;//局部坐标系中的y坐标
                                                double delta = angle * Math.PI / 180;

                                                leader_x_coordinate = follower_x_coordinate + Math.Cos(delta) * x - Math.Sin(delta) * y;
                                                leader_y_coordinate = follower_y_coordinate + Math.Sin(delta) * x + Math.Cos(delta) * y;

                                                if (!(vehicle_following.vehicle_control == null || vehicle_following.vehicle_control.IsDisposed))
                                                {
                                                    if (vehicle_following.vehicle_control.control_mode == ControlMode.Trajectory)
                                                    {
                                                        if(vehicle_following.vehicle_control.PointLength(new TrajectoryPoint(leader_x_coordinate,leader_y_coordinate), 
                                                            new TrajectoryPoint(leader_x_coordinate_temp2,leader_y_coordinate_temp2)) > vehicle_following.vehicle_control.coeff_trajectory_PointThreshold 
                                                            && vehicle_following.target_dataupdate_Camera)
                                                        {
                                                            vehicle_following.vehicle_control.leaderPointStore.Enqueue(new TrajectoryPoint(leader_x_coordinate, leader_y_coordinate));
                                                            if (vehicle_following.vehicle_control.leaderPointStore.Count >= 2)
                                                            {
                                                                vehicle_following.vehicle_control.trajectory_length += vehicle_following.vehicle_control.PointLength(
                                                                vehicle_following.vehicle_control.leaderPointStore.ElementAt<TrajectoryPoint>(vehicle_following.vehicle_control.leaderPointStore.Count - 1),
                                                                vehicle_following.vehicle_control.leaderPointStore.ElementAt<TrajectoryPoint>(vehicle_following.vehicle_control.leaderPointStore.Count - 2));
                                                            }
                                                            vehicle_following.vehicle_control.chart_TrajectoryState.BeginInvoke(new EventHandler(delegate
                                                            {
                                                                vehicle_following.vehicle_control.chart_TrajectoryState.Series[0].Points.AddXY(leader_x_coordinate, leader_y_coordinate);
                                                                vehicle_following.vehicle_control.ChangeChartAxisRange(leader_x_coordinate, leader_y_coordinate);
                                                            }));
                                                            leader_x_coordinate_temp2 = leader_x_coordinate;
                                                            leader_y_coordinate_temp2 = leader_y_coordinate;
                                                        }
                                                    }
                                                }
                                            }
                                        }

                                        count = 0;
                                        flag_start = false;
                                        break;
                                    }
                                    else
                                    {
                                        if (FollowerVehicle_open)
                                        {
                                            followervehicle.richTextBox_show.Invoke(new EventHandler(delegate
                                            {
                                                followervehicle.richTextBox_show.SelectionColor = Color.Red;
                                                followervehicle.richTextBox_show.AppendText("ERROR: 校验码错误!" + "\n");
                                                followervehicle.richTextBox_show.ScrollToCaret();
                                            }));
                                        }
                                        richTextBox_DataShow.Invoke(new EventHandler(delegate
                                        {
                                            richTextBox_DataShow.SelectionColor = Color.Red;
                                            richTextBox_DataShow.AppendText("ERROR: 校验码错误!" + "\n");
                                            richTextBox_DataShow.ScrollToCaret();
                                        }));
                                        count = 0;
                                        flag_start = false;
                                        break;
                                    }
                                }
                            }
                        }

                        data_temp = data;
                        goto READ_BYTE;

                        #endregion

                    case ReceiveState.LMS111DataPacket:
                        #region LMS111测试
                    ReadData_LMS: data1 = serialport.ReadByte();
                        data = (byte)data1;

                        if (data == 0x02)
                        {
                            flag_data = true;
                        }
                        if (flag_data)
                        {
                            if (data1 == 0x03)
                            {
                                //显示
                                lms111.richTextBox_GetData.BeginInvoke(new EventHandler(delegate
                                {
                                    if (LMS111_open)
                                    {
                                        for (int i = 0; i < LMS_NUM; i++)
                                        {
                                            lms111.richTextBox_GetData.SelectionColor = Color.Black;
                                            lms111.richTextBox_GetData.AppendText("[" + i.ToString() + "] ");
                                            lms111.richTextBox_GetData.SelectionColor = Color.Blue;
                                            lms111.richTextBox_GetData.AppendText(LMS_data[i].ToString());
                                            lms111.richTextBox_GetData.SelectionColor = Color.Purple;
                                            lms111.richTextBox_GetData.AppendText(" mm\n");
                                            lms111.richTextBox_GetData.ScrollToCaret();
                                        }
                                    }
                                }));

                                richTextBox_DataShow.BeginInvoke(new EventHandler(delegate
                                {
                                    for (int i = 0; i < LMS_NUM; i++)
                                    {
                                        richTextBox_DataShow.SelectionColor = Color.Black;
                                        richTextBox_DataShow.AppendText("[" + i.ToString() + "] ");
                                        richTextBox_DataShow.SelectionColor = Color.Blue;
                                        richTextBox_DataShow.AppendText(LMS_data[i].ToString());
                                        richTextBox_DataShow.SelectionColor = Color.Purple;
                                        richTextBox_DataShow.AppendText(" mm\n");
                                        richTextBox_DataShow.ScrollToCaret();
                                    }
                                }));
                                
                                break;
                            }
                            else
                            {
                                if ((data == 0x20) && (flag_start_lms == false))
                                {
                                    count1++;
                                }
                                if (count1 >= 26)
                                {
                                    flag_start_lms = true;
                                }

                                if ((count3 < LMS_NUM) && flag_start_lms)
                                {
                                    if (data == 0x20)
                                    {
                                        switch (count2)
                                        {
                                            case 1:
                                                LMS_data[count3] = temp[0];
                                                count3++;
                                                count2 = 0;
                                                break;

                                            case 2:
                                                LMS_data[count3] = (temp[0] << 4) + temp[1];
                                                count3++;
                                                count2 = 0;
                                                break;

                                            case 3:
                                                LMS_data[count3] = (temp[0] << 8) + (temp[1] << 4) + temp[2];
                                                count3++;
                                                count2 = 0;
                                                break;

                                            case 4:
                                                LMS_data[count3] = (temp[0] << 12) + (temp[1] << 8) + (temp[2] << 4) + temp[3];
                                                count3++;
                                                count2 = 0;
                                                break;

                                            case 5:
                                                LMS_data[count3] = (temp[0] << 16) + (temp[1] << 12) + (temp[2] << 8) + (temp[3] << 4) + temp[4];
                                                count3++;
                                                count2 = 0;
                                                break;

                                            case 6:
                                                LMS_data[count3] = (temp[0] << 20) + (temp[1] << 16) + (temp[2] << 12) + (temp[3] << 8) + (temp[4] << 4) + temp[5];
                                                count3++;
                                                count2 = 0;
                                                break;

                                            default:

                                                break;
                                        }
                                    }
                                    else
                                    {
                                        if ((data >= 0x30) && (data <= 0x39))
                                        {
                                            temp[count2] = (byte)(data - 0x30);
                                        }
                                        else if ((data >= 0x41) && (data <= 0x46))
                                        {
                                            temp[count2] = (byte)(data - 0x37);
                                        }
                                        count2++;
                                    }
                                }
                                data_temp = data;
                                goto ReadData_LMS;
                            }
                        }
                        break;
                        #endregion

                    case ReceiveState.LMS111SettingPacket:
                        #region  LMS111测试 直接显示ASCII或16进制

                    ReadData_LMS111_R: data1 = serialport.ReadByte();
                        data = (byte)data1;

                        if (flag_ASCII_r)
                        {
                            //用ASCII显示
                            if (LMS111_open)
                            {
                                lms111.richTextBox_ReceiveData.Invoke(new EventHandler(delegate
                                {
                                    lms111.richTextBox_ReceiveData.SelectionColor = Color.Black;
                                    lms111.richTextBox_ReceiveData.AppendText(ConvertToASCII_One(data));
                                    lms111.richTextBox_ReceiveData.ScrollToCaret();
                                }));
                            }
                            richTextBox_DataShow.Invoke(new EventHandler(delegate
                            {
                                richTextBox_DataShow.SelectionColor = Color.Black;
                                richTextBox_DataShow.AppendText(ConvertToASCII_One(data));
                                richTextBox_DataShow.ScrollToCaret();
                            }));
                        }
                        else
                        {
                            //用十六进制显示
                            if (LMS111_open)
                            {
                                lms111.richTextBox_ReceiveData.Invoke(new EventHandler(delegate
                                {
                                    lms111.richTextBox_ReceiveData.SelectionColor = Color.Black;
                                    lms111.richTextBox_ReceiveData.AppendText(Convert.ToString(data, 16) + " ");
                                    lms111.richTextBox_ReceiveData.ScrollToCaret();
                                }));
                            }
                            //用十六进制显示
                            richTextBox_DataShow.Invoke(new EventHandler(delegate
                            {
                                richTextBox_DataShow.SelectionColor = Color.Black;
                                richTextBox_DataShow.AppendText(Convert.ToString(data, 16) + " ");
                                richTextBox_DataShow.ScrollToCaret();
                            }));
                        }

                        if (data == 0x03)
                        {
                            break;
                        }
                        goto ReadData_LMS111_R;
                        #endregion

                    case ReceiveState.OnlyServoPacket:
                        #region 单个舵机测试
                    ReadData_Servo: 
                        data1 = serialport.ReadByte();
                        data = (byte)data1;

                        if ((data == 0xFF) && (data_temp == 0xFF) && (!flag_start_svo))
                        {
                            data_receive[0] = 0xFF;
                            data_receive[1] = 0xFF;
                            count_svo = 2;
                            flag_start_svo = true;
                        }
                        else if (flag_start_svo)
                        {
                            if (count_svo == 2)
                            {
                                data_receive[count_svo] = data;
                                count_svo++;
                            }
                            else if (count_svo == 3)
                            {
                                data_length = data + 4;
                                data_receive[count_svo] = data;
                                count_svo++;
                                if (data_length > 20)  //过滤错误包,防止错误累积,和读内存的错误！！
                                {
                                    flag_start_svo = false;

                                    if (FollowerVehicle_open)
                                    {
                                        followervehicle.richTextBox_show.Invoke(new EventHandler(delegate
                                        {
                                            followervehicle.richTextBox_show.SelectionColor = Color.Red;
                                            followervehicle.richTextBox_show.AppendText("ERROR: 数据包不合理!" + "\n");
                                            followervehicle.richTextBox_show.ScrollToCaret();
                                        }));
                                    }
                                    richTextBox_DataShow.Invoke(new EventHandler(delegate
                                    {
                                        richTextBox_DataShow.SelectionColor = Color.Red;
                                        richTextBox_DataShow.AppendText("ERROR: 数据包不合理!" + "\n");
                                        richTextBox_DataShow.ScrollToCaret();
                                    }));
                                }
                            }
                            else
                            {
                                if (count_svo < data_length)
                                {
                                    data_receive[count_svo] = data;
                                    count_svo++;
                                }
                                if (count_svo == data_length)
                                {
                                    for (int i = 2; i < data_length; i++)
                                    {
                                        checksum += data_receive[i];
                                    }

                                    //数据包检验正确
                                    if (checksum == 0xFF)
                                    {
                                        //数据包长度为16
                                        if (data_length == 16)
                                        {
                                           
                                        }

                                        //显示所接收的正确数据包
                                        if (FollowerVehicle_open)
                                        {
                                            followervehicle.richTextBox_show.Invoke(new EventHandler(delegate
                                            {
                                                followervehicle.richTextBox_show.SelectionColor = Color.Red;
                                                followervehicle.richTextBox_show.AppendText(ByteArrayToHexString_length(data_receive, data_length) + "\n");
                                                followervehicle.richTextBox_show.ScrollToCaret();
                                            }));
                                        }
                                        richTextBox_DataShow.Invoke(new EventHandler(delegate
                                        {
                                            richTextBox_DataShow.SelectionColor = Color.Red;
                                            richTextBox_DataShow.AppendText(ByteArrayToHexString_length(data_receive, data_length) + "\n");
                                            richTextBox_DataShow.ScrollToCaret();
                                        }));
                                        count_svo = 0;
                                        flag_start_svo = false;
                                        break;
                                    }
                                    else
                                    {
                                        if (FollowerVehicle_open)
                                        {
                                            followervehicle.richTextBox_show.Invoke(new EventHandler(delegate
                                            {
                                                followervehicle.richTextBox_show.SelectionColor = Color.Red;
                                                followervehicle.richTextBox_show.AppendText("ERROR: 校验码错误!" + "\n");
                                                followervehicle.richTextBox_show.ScrollToCaret();
                                            }));
                                        }
                                        richTextBox_DataShow.Invoke(new EventHandler(delegate
                                        {
                                            richTextBox_DataShow.SelectionColor = Color.Red;
                                            richTextBox_DataShow.AppendText("ERROR: 校验码错误!" + "\n");
                                            richTextBox_DataShow.ScrollToCaret();
                                        }));
                                        count_svo = 0;
                                        flag_start_svo = false;
                                        break;
                                    }
                                }
                                
                            }
                        }

                        data_temp = data;
                        goto ReadData_Servo;
                        #endregion
                    default:

                        break;
                }
            }
            //通讯指示灯绿
            flag_communicate_show = true;

        }

        private void Time_count_tick(object sender, ElapsedEventArgs e) //200ms
        {
            if (flag_communicate_show)
            {
                if (!flag_communicate_green)
                {
                    toolStripStatusLabel_state_communicate.Image = Image.FromFile(path + "/image/green.jpg");
                    flag_communicate_green = true;
                }
                flag_communicate_show = false;
            }
            else
            {
                if (flag_communicate_green)
                {
                    toolStripStatusLabel_state_communicate.Image = Image.FromFile(path + "/image/red.jpg");
                    flag_communicate_green = false;
                }
            }
        }

        private void VehicleTrack_tick(object sender, ElapsedEventArgs e)   //50ms
        {
            if ((leader_x_coordinate != leader_x_coordinate_temp) || (leader_y_coordinate != leader_y_coordinate_temp))
            {
                toolStripStatusLabel_LXValue.Text = ((Int32)leader_x_coordinate).ToString();
                toolStripStatusLabel_LYValue.Text = ((Int32)leader_y_coordinate).ToString();

                if (!(vehicle_following == null || vehicle_following.IsDisposed))
                {
                    if (vehicle_following.serial_receive_on)
                    {
                        Int32 lx = (Int32)leader_x_coordinate;
                        Int32 ly = (Int32)leader_y_coordinate;
                        chart_VehicleFollowing.BeginInvoke(new EventHandler(delegate
                        {
                            chart_VehicleFollowing.Series[0].Points.AddXY(lx, ly);
                        }));
                        
                    }
                }
            }
            leader_x_coordinate_temp = leader_x_coordinate;
            leader_y_coordinate_temp = leader_y_coordinate;

            if ((follower_x_coordinate != follower_x_coordinate_temp) || (follower_y_coordinate != follower_y_coordinate_temp))
            {
                toolStripStatusLabel_xValue.Text = follower_x_coordinate.ToString();
                toolStripStatusLabel_yValue.Text = follower_y_coordinate.ToString();
                toolStripStatusLabel_speedValue.Text = speed.ToString();
                toolStripStatusLabel_angleValue.Text = angle.ToString();
                
                Int32 fx = (Int32)follower_x_coordinate;
                Int32 fy = (Int32)follower_y_coordinate;
                chart_VehicleFollowing.BeginInvoke(new EventHandler(delegate
                {
                    chart_VehicleFollowing.Series[1].Points.AddXY(fx, fy);
                }));
                
            }
            follower_x_coordinate_temp = follower_x_coordinate;
            follower_y_coordinate_temp = follower_y_coordinate;

            double xmax = Math.Max(follower_x_coordinate, leader_x_coordinate);
            double xmin = Math.Min(follower_x_coordinate, leader_x_coordinate);
            double ymax = Math.Max(follower_y_coordinate, leader_y_coordinate);
            double ymin = Math.Min(follower_y_coordinate, leader_y_coordinate);

            //调整坐标轴
            chart_VehicleFollowing.BeginInvoke(new EventHandler(delegate
            {
                if (xmax > (XMAX - (XMAX - XMIN) * 0.05))
                {
                    XMAX = (xmax - 0.05 * XMIN) / 0.95;

                    chart_VehicleFollowing.ChartAreas[0].AxisX.Maximum = XMAX;
                    this.BeginInvoke(new EventHandler(delegate
                    {
                        toolStripTextBox_x_max.Text = ((Int32)XMAX).ToString();
                    }));

                }
                if (xmin < (XMIN + (XMAX - XMIN) * 0.05))
                {
                    XMIN = (xmin - 0.05 * XMAX) / 0.95;
                    chart_VehicleFollowing.ChartAreas[0].AxisX.Minimum = XMIN;
                    this.BeginInvoke(new EventHandler(delegate
                    {
                        toolStripTextBox_x_min.Text = ((Int32)XMIN).ToString();
                    }));
                }
                if (ymax > (YMAX - (YMAX - YMIN) * 0.05))
                {
                    YMAX = (ymax - 0.05 * YMIN) / 0.95;
                    chart_VehicleFollowing.ChartAreas[0].AxisY.Maximum = YMAX;
                    this.BeginInvoke(new EventHandler(delegate
                    {
                        toolStripTextBox_y_max.Text = ((Int32)YMAX).ToString();
                    }));
                }
                if (ymin < (YMIN + (YMAX - YMIN) * 0.05))
                {
                    YMIN = (ymin - 0.05 * YMAX) / 0.95;
                    chart_VehicleFollowing.ChartAreas[0].AxisY.Minimum = YMIN;
                    this.BeginInvoke(new EventHandler(delegate
                    {
                        toolStripTextBox_y_min.Text = ((Int32)YMIN).ToString();
                    }));
                }
            }));
            
        }

        private void sendmore_tick(object sender, ElapsedEventArgs e) //150ms
        {
            vehicle_mw.VehicleControl((byte)int.Parse(toolStripTextBox_state.Text), int.Parse(toolStripTextBox_speed.Text),
                                        int.Parse(toolStripTextBox_servo.Text), (byte)int.Parse(toolStripTextBox_light.Text));
        }

        #endregion        
    }
}
